#!/usr/bin/env python3

import time
import rclpy
from rclpy.node import Node
from manipulation.scripts.detection.commans.base_detection_node import BaseDetectionNode
from manipulation.msg import ManiProcessCmd, TargetsList
from copy import deepcopy
from geometry_msgs.msg import TransformStamped
from threading import Thread

class ManiConnectorNode(Node):
    def __init__(self, detection_node: BaseDetectionNode):
        super().__init__('mani_connector_node')
        self.declare_parameter('hz', 10)  # Frequency for publishing target TF
        self.hz = self.get_parameter('hz').get_parameter_value().integer_value
        self.detection_node = detection_node
        self.pub_process = self.create_publisher(ManiProcessCmd, 'mani_process', 1)  # Publish to ROS1
        self.pub_detection_targets = self.create_publisher(
            TargetsList, 'detection_targets', 1
        )
        self.sub_process2 = self.create_subscription(
            ManiProcessCmd, 'mani_process2', self.process_callback, 1
        )  # Subscribe from ROS2
        self.reset()
        self.thread = Thread(target=self.update_loop, daemon=True)
        self.thread.start()
        self.get_logger().info("ManiConnectorNode initialized")
    
    def process_callback(self, msg: ManiProcessCmd):
        """ Process the callback from the detection node. """
        self.get_logger().info(f"Processing command: level_name1={msg.level1_name}, level_name2={msg.level2_name}, value={msg.value}")
        if msg.level1_name == 'start':
            idx = int(msg.value)
            if 0 <= idx < len(self.targets_list):
                self.current_target_idx = idx
                self.get_logger().info(f"Start grasping target {idx}: {self.targets_list[idx]}")
                self.pub_process.publish(ManiProcessCmd(level1_name='start', level2_name=self.targets_list[idx]['child_frame_id']))
            else:
                self.get_logger().error(f"Target index {idx} out of range. Available targets: 0~{len(self.targets_list)-1}")
        elif msg.level1_name == 'reset':
            self.pub_process.publish(ManiProcessCmd(level1_name='reset'))
        elif msg.level1_name == 'quit':
            self.pub_process.publish(ManiProcessCmd(level1_name='quit'))
        elif msg.level1_name == 'pause':
            self.pub_process.publish(ManiProcessCmd(level1_name='pause'))
        elif msg.level1_name == 'prepare':
            self.pub_process.publish(ManiProcessCmd(level1_name='task0', level2_name='head_down'))
            time.sleep(2.0)  # 等待相机稳定后, 开始移动到准确抓取位置
            self.pub_process.publish(ManiProcessCmd(level1_name='move', level2_name='target_0'))
            # self.pub_process.publish(ManiProcessCmd(level1_name='move', level2_name='refer'))  # DEBUG: 先对齐refer frame测试
        elif msg.level1_name == 'place':
            cmd = ManiProcessCmd(level1_name='place', level2_name='origin')
            if msg.level2_name == 'move':
                cmd.level2_name = 'left' if msg.value else 'right'  # 好的放left, 坏的放right
            self.pub_process.publish(cmd)
        else:
            self.get_logger().error(f"Unknown command: {msg.level1_name}. Supported commands: ['start', 'reset', 'quit', 'pause', 'prepare', 'place']")
    
    def update_loop(self):
        rate = self.create_rate(self.hz)
        while True:
            # if self.current_target_idx is not None:  # target_0已经由detection_node在detect循环中发送出去了, 且目前仅抓取target_0, 此处无需再次发送
            #     self.detection_node.publish_tf_from_detect_info(
            #         child_frame_id='grab_target',
            #         info=self.targets_list[self.current_target_idx]
            #     )
            self.targets_list = deepcopy(self.detection_node.targets_list)
            if self.targets_list is not None:
                targets_msg = TargetsList(
                    names=[target['child_frame_id'] for target in self.targets_list],
                    grasped=[False for _ in range(len(self.targets_list))]
                )
                self.pub_detection_targets.publish(targets_msg)
            rate.sleep()
    
    def reset(self):
        self.targets_list = None  # Store detected targets from the detection node at the first start
        self.current_target_idx = None

if __name__ == '__main__':
    rclpy.init()
    detection_node = BaseDetectionNode()
    node = ManiConnectorNode(detection_node)
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()
